/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
#include "fc_sensors.h"
#include "filter.h"

// 定义卡尔曼滤波器结构体
#include <stdio.h>

typedef struct {
    float x[2];  // 状态向量 [位置; 速度]
    float P[2][2];  // 状态协方差矩阵
    float Q[2][2];  // 过程噪声协方差矩阵
    float R;        // 测量噪声协方差
    float F[2][2];  // 状态转移矩阵
    float H[2];     // 观测矩阵
    float K[2];     // 卡尔曼增益
} KalmanFilter2_t;

void KalmanFilter_init(KalmanFilter2_t *kf, float dt) {
    // 初始化状态向量
    kf->x[0] = 0;
    kf->x[1] = 0;

    // 初始化状态协方差矩阵
    kf->P[0][0] = 1;
    kf->P[0][1] = 0;
    kf->P[1][0] = 0;
    kf->P[1][1] = 1;

    // 初始化过程噪声协方差矩阵
    kf->Q[0][0] = 0.01;
    kf->Q[0][1] = 0;
    kf->Q[1][0] = 0;
    kf->Q[1][1] = 0.01;

    // 初始化测量噪声协方差
    kf->R = 1;

    // 初始化状态转移矩阵
    kf->F[0][0] = 1;
    kf->F[0][1] = dt;
    kf->F[1][0] = 0;
    kf->F[1][1] = 1;

    // 初始化观测矩阵
    kf->H[0] = 1;
    kf->H[1] = 0;
}

void KalmanFilter_predict(KalmanFilter2_t *kf) {
    // 预测状态
    float x0 = kf->x[0];
    float x1 = kf->x[1];
    kf->x[0] = kf->F[0][0] * x0 + kf->F[0][1] * x1;
    kf->x[1] = kf->F[1][0] * x0 + kf->F[1][1] * x1;

    // 预测协方差
    float P00 = kf->P[0][0];
    float P01 = kf->P[0][1];
    float P10 = kf->P[1][0];
    float P11 = kf->P[1][1];

    kf->P[0][0] = kf->F[0][0] * P00 + kf->F[0][1] * P10;
    kf->P[0][1] = kf->F[0][0] * P01 + kf->F[0][1] * P11;
    kf->P[1][0] = kf->F[1][0] * P00 + kf->F[1][1] * P10;
    kf->P[1][1] = kf->F[1][0] * P01 + kf->F[1][1] * P11;

    // 添加过程噪声
    kf->P[0][0] += kf->Q[0][0];
    kf->P[0][1] += kf->Q[0][1];
    kf->P[1][0] += kf->Q[1][0];
    kf->P[1][1] += kf->Q[1][1];
}

void KalmanFilter_update(KalmanFilter2_t *kf, float z) {
    // 计算卡尔曼增益
    float S = kf->H[0] * kf->P[0][0] * kf->H[0] + kf->H[1] * kf->P[1][0] * kf->H[0] + kf->R;
    kf->K[0] = kf->P[0][0] * kf->H[0] / S;
    kf->K[1] = kf->P[1][0] * kf->H[0] / S;

    // 更新状态估计
    float y = z - (kf->H[0] * kf->x[0] + kf->H[1] * kf->x[1]);
    kf->x[0] += kf->K[0] * y;
    kf->x[1] += kf->K[1] * y;

    // 更新协方差估计
    float P00 = kf->P[0][0];
    float P01 = kf->P[0][1];
    float P10 = kf->P[1][0];
    float P11 = kf->P[1][1];

    kf->P[0][0] = (1 - kf->K[0] * kf->H[0]) * P00;
    kf->P[0][1] = (1 - kf->K[0] * kf->H[0]) * P01;
    kf->P[1][0] = -kf->K[1] * kf->H[0] * P10 + P10;
    kf->P[1][1] = -kf->K[1] * kf->H[0] * P11 + P11;
}
#ifdef TEST_KALMAN
int main() {
    KalmanFilter2_t kf;
    float dt = 0.1;  // 时间步长
    KalmanFilter_init(&kf, dt);

    float measurements[] = {1, 2, 3, 4, 5};  // 测量值
    int n = sizeof(measurements) / sizeof(measurements[0]);

    for (int i = 0; i < n; i++) {
        KalmanFilter_predict(&kf);
        KalmanFilter_update(&kf, measurements[i]);
        printf("Estimate: %f, %f\n", kf.x[0], kf.x[1]);
    }

    return 0;
}
#endif
int baro_fusion(void)
{
    // static uint32_t last_time = 0;
    // uint32_t current_time = rt_tick_get();
    // float dt = (float)(current_time - last_time) * 0.001f;
    // static float baro_last_height = 0;
    // if (!baro_kf_init)
    // {
    //     baro_kf_init = true;
    //     KalmanFilter_Init(&baro_kf, 0, 0);
    // }

    // KalmanFilter_Predict(&baro_kf, zdrone_state.acc.x * 980, dt);
    // // 更新步骤
    // KalmanFilter_Update(&baro_kf, fc_sensor_data.baro.altitude);

    // float fusion_height = baro_kf.x[0];
    // float fusion_velocity = baro_kf.x[1];
    // float vz = (fc_sensor_data.baro.altitude - baro_last_height) / dt;
    // kalman_fusion_z(zdrone_state.acc.z * 980, vz, dt);
    // zdrone_state.pos.z = fusion_height;
    // zdrone_state.vel.z = fusion_velocity;
    return fc_sensor_data.baro.altitude;
}
